#ifndef SLAM_ROS_SLAM_ROS_MAP_BUILDER_BRIDGE_H
#define SLAM_ROS_SLAM_ROS_MAP_BUILDER_BRIDGE_H

#include <memory>
#include <set>
#include <string>
#include <unordered_map>
#include <functional>

#include "absl/synchronization/mutex.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "slam/mapping/map_builder_interface.h"
#include "slam/mapping/pose_graph_interface.h"
#include "slam/mapping/proto/trajectory_builder_options.pb.h"
#include "slam/mapping/trajectory_builder_interface.h"
#include "slam_ros/node_options.h"
#include "slam_ros/sensor_bridge.h"
#include "slam_ros/tf_bridge.h"
#include "slam_ros/trajectory_options.h"
#include "slam_ros_msgs/msg/submap_entry.hpp"
#include "slam_ros_msgs/msg/submap_list.hpp"
#include "slam_ros_msgs/srv/submap_query.hpp"
#include "slam_ros_msgs/srv/trajectory_query.hpp"

#ifdef DELETE
#undef DELETE
#endif
#include "visualization_msgs/msg/marker_array.hpp"

namespace slam_ros {

using PosePublishCallback = std::function<void(
    ::slam::common::Time, 
    ::slam::transform::Rigid3d,
    ::slam::transform::Rigid3d, 
    ::slam::mapping::ReflectorMatchResult)>;

class MapBuilderBridge {
 public:
  struct LocalTrajectoryData {
    struct LocalSlamData {
      ::slam::common::Time time;
      ::slam::transform::Rigid3d local_pose;
      ::slam::mapping::ReflectorMatchResult reflector_match_result;
      ::slam::sensor::RangeData range_data_in_local;
    };
    std::shared_ptr<const LocalSlamData> local_slam_data;
    slam::transform::Rigid3d local_to_map;
    std::unique_ptr<slam::transform::Rigid3d> published_to_tracking;
    TrajectoryOptions trajectory_options;
  };

  MapBuilderBridge(
      const NodeOptions& node_options,
      std::unique_ptr<slam::mapping::MapBuilderInterface> map_builder,
      tf2_ros::Buffer* tf_buffer);

  MapBuilderBridge(const MapBuilderBridge&) = delete;
  MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;

  void LoadState(const std::string& state_filename, bool load_frozen_state);
  int AddTrajectory(
      const std::set<::slam::mapping::TrajectoryBuilderInterface::SensorId>&
          expected_sensor_ids,
      const TrajectoryOptions& trajectory_options);
  void FinishTrajectory(int trajectory_id);
  void RunFinalOptimization();
  bool SerializeState(const std::string& filename, 
                     const bool cover_old_map,
                     const bool include_unfinished_submaps);

  void HandleSubmapQuery(
      const std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Response> response);
  
  void HandleTrajectoryQuery(
      const std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Request> request,
      std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Response> response);

  std::map<int, ::slam::mapping::PoseGraphInterface::TrajectoryState>
  GetTrajectoryStates();
  
  slam_ros_msgs::msg::SubmapList GetSubmapList(::rclcpp::Time node_time);
  
  std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
      LOCKS_EXCLUDED(mutex_);
  
  visualization_msgs::msg::MarkerArray GetTrajectoryNodeList(::rclcpp::Time node_time);
  visualization_msgs::msg::MarkerArray GetLandmarkPosesList(::rclcpp::Time node_time);
  visualization_msgs::msg::MarkerArray GetConstraintList(::rclcpp::Time node_time);

  SensorBridge* sensor_bridge(int trajectory_id);

  bool IsLocalizationSuccess();
  float GetMeanMatchScore();
  void SetOnlyUseOdomPredictPoseFlag(const bool flag);
  std::string PgmToLocalizationMap(const std::string& pgm_file_path);
  bool SavePoseGraphAndMap(const std::string& map_name);
  bool QueryFontLaserDataStatus();
  void SetPosePublishCallback(PosePublishCallback call_back);
  void ResetMapBuilder(const NodeOptions& node_options);

 private:
  void OnLocalSlamResult(
      const int trajectory_id, 
      const ::slam::common::Time time,
      const ::slam::transform::Rigid3d local_pose,
      const slam::mapping::ReflectorMatchResult reflector_match_result,
      ::slam::sensor::RangeData range_data_in_local) LOCKS_EXCLUDED(mutex_);

  absl::Mutex mutex_;
  const NodeOptions node_options_;
  std::unordered_map<int, 
                    std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
      local_slam_data_ GUARDED_BY(mutex_);
  std::unique_ptr<slam::mapping::MapBuilderInterface> map_builder_;
  tf2_ros::Buffer* const tf_buffer_;

  std::unordered_map<std::string, int> landmark_to_index_;
  std::unordered_map<int, TrajectoryOptions> trajectory_options_;
  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
  std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
  PosePublishCallback pose_publish_callback_;
};

}  // namespace slam_ros

#endif